#ifndef __SV2_GIMBAL_SMS__
#define __SV2_GIMBAL_SMS__

#include "sms_core.h"
#include <string>
#include <chrono>
#include <mutex>
#include <queue>
#include <condition_variable>
#include <opencv2/opencv.hpp>
#include "sv2_gimbal.h"

namespace sv2
{
  enum QoS
  {
    BestEffort = 1,
    Reliability = 2
  };

  // 定义吊舱
  sv2::Gimbal *gimbal;

  nlohmann::json gimbal_state_msg = sms::def_msg("spirecv_msgs::GimbalState");

  typedef struct
  {
    sv2::GimbalType type;
    sv2::GimbalLink link;
  } SV2_GIMBAL_TYPE_T;

  typedef std::map<std::string, SV2_GIMBAL_TYPE_T> typeMap;
  typeMap sv2GimbalTypeList =
      {
          {"G1", {sv2::GimbalType::G1, sv2::GimbalLink::SERIAL}},
          {"Q10f", {sv2::GimbalType::Q10f, sv2::GimbalLink::SERIAL}},
          {"GX40", {sv2::GimbalType::GX40, sv2::GimbalLink::SERIAL}},
          {"SU17", {sv2::GimbalType::SU17, sv2::GimbalLink::ETHERNET_UDP2}},
  };

  SV2_GIMBAL_TYPE_T findSVGimbal(const std::string &type)
  {
    typeMap::iterator temp = sv2GimbalTypeList.find(type);

    if (temp != sv2GimbalTypeList.end())
    {
      return (temp->second);
    }
    std::cout << type << " is Unsupported device type!" << std::endl;
    exit(-1);
  }

  // gimbal state callback
  void updateState(double &frameAngleRoll, double &frameAnglePitch, double &frameAngleYaw,
                   double &imuAngleRoll, double &imuAnglePitch, double &imuAngleYaw,
                   double &fovX, double &fovY)
  {
    gimbal_state_msg["angle_euler"] = {imuAngleRoll, imuAnglePitch, frameAngleYaw};
    gimbal_state_msg["fov"] = {fovX, fovY};
  }

  class GimbalCppNode : public sms::BaseNode
  {
  public:
    GimbalCppNode(
        std::string job_name,
        std::string param_file,
        QoS qos = QoS::Reliability,
        std::string ip = "127.0.0.1",
        int port = 9094) : sms::BaseNode("GimbalCppNode", job_name, param_file, ip, port),
                           _gimbal_ctl_sub("/" + job_name + "/gimbal/control", "spirecv_msgs::GimbalControl", std::bind(&GimbalCppNode::gimbalctl_callback, this, std::placeholders::_1)),
                           _gimbal_state_pub("/" + job_name + "/gimbal/state", "spirecv_msgs::GimbalState")
    {
      // 读取节点参数
      std::string gimbalType = this->get_param("gimbal_type", "G1");
      std::string serialPotr = this->get_param("serial_potr", "/dev/ttyTHS1");

      logger.info("gimbalType: " + gimbalType);
      logger.info("serialPotr: " + serialPotr);

      // 实例化吊舱
      gimbal = new sv2::Gimbal(findSVGimbal(gimbalType).type, findSVGimbal(gimbalType).link);
      if (findSVGimbal(gimbalType).link == sv2::GimbalLink::SERIAL)
      {
        // 使用 /dev/ttyUSB0 作为控制串口
        gimbal->setSerialPort(serialPotr);
      }
      else if (findSVGimbal(gimbalType).link == sv2::GimbalLink::ETHERNET_UDP2)
      {
        // 使用 默认参数
        // gimbal->setNetIp("192.168.1.99");
        // gimbal->setUdpNetPort(51121,51121);
      }
      else
      {
        exit(-1);
      }

      gimbal->open(updateState);

      gimbal_state_msg["gb_type"] = (uint8_t)findSVGimbal(gimbalType).type;
      gimbal_state_msg["move_mode"] = 3;
      gimbal_state_msg["image_mode"] = 0;
      gimbal_state_msg["is_zoom"] = 0;
      gimbal_state_msg["rec_mode"] = 0;
    }
    ~GimbalCppNode()
    {
    }

    void run();

    // gimbalctl参数话题回调函数
    void gimbalctl_callback(nlohmann::json msg)
    {

      this->angle_euler_cmd = msg["angle_euler_cmd"];
      this->angle_euler_rate_cmd = msg["angle_euler_rate_cmd"];

      this->ctl_mode = msg["ctl_mode"];
      this->rec_mode = msg["rec_mode"];
      this->zoom = msg["zoom"];
    }

    nlohmann::json angle_euler_cmd;
    nlohmann::json angle_euler_rate_cmd;
    int ctl_mode;
    int rec_mode;
    double zoom;
    // sv2::Gimbal *gimbal;

  private:
    // 订阅话题
    sms::Subscriber _gimbal_ctl_sub;
    // 发布话题
    sms::Publisher _gimbal_state_pub;

    std::mutex _ctl_queue_mtx;
    std::queue<nlohmann::json> _ctl_queue;
    std::condition_variable _gimbal_ctl;
  };
}
#endif
